#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>

#define DEBUG


using namespace cv;

int hueMin = 34;
int hueMax = 79;

int satMin = 133;
int satMax = 255;

int valMin = 63;
int valMax = 255;

int areaValue = 500;



CvMemStorage* contourStorage = NULL;

IplImage* frame = NULL;
IplImage* imgHSV = NULL;
IplImage* imgThreshed = NULL;

#ifdef DEBUG
CvScalar colorWhite = CV_RGB(255,255,255);
CvScalar colorRed = CV_RGB(255,0,0);
CvScalar colorGreen = CV_RGB(0, 255, 0);
CvScalar colorBlue = CV_RGB(0,0,255);
CvScalar colorYellow = CV_RGB(255,255,0);

#endif

int numPoints = 0;

int topLeftIndex = 0;

int boundingBoxSearchTolerance = 55;
CvPoint2D32f cornerArray[4];
CvPoint tempPoint = cvPoint(0,0);


void findStuff(int)
{
	if (contourStorage == NULL) //intitiaize stuff it it's not there already
	{
		contourStorage = cvCreateMemStorage(0);
		imgHSV = cvCreateImage(cvGetSize(frame), 8, 3);
		imgThreshed = cvCreateImage(cvGetSize(frame), 8, 1);
	}
	else
	{
		cvClearMemStorage(contourStorage);
	}

	CvSeq* contours = NULL;

	cvCvtColor(frame, imgHSV, CV_BGR2HSV);

	cvInRangeS(imgHSV, cvScalar(hueMin, satMin, valMin), cvScalar(hueMax, satMax, valMax), imgThreshed);

	cvShowImage("Threshold Result", imgThreshed);

	//begin corner detection

	cvFindContours(imgThreshed, contourStorage, &contours);

	while (contours!=0)
	{

		cvDrawContours(imgThreshed, contours, colorWhite, colorWhite, -1, 2, 8);
		//cvShowImage("Contourizable Crap", imgThreshed);


		double area = cvContourArea(contours);

		if (area > areaValue && !CV_IS_SEQ_HOLE(contours))
		{
			//get a series of points on the contour i the area inside the contour is greater than a threshold
			CvSeq* contourPoints = cvApproxPoly(contours, sizeof(CvContour), contourStorage, CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0); //sliderize the 0.02
			contourPoints = cvConvexHull2(contourPoints, 0, CV_CLOCKWISE, 1);
			numPoints = contourPoints->total;


			CvScalar selectColor = CV_RGB(0,0,0);
			CvPoint pts[numPoints];

			for (int i=0; i<numPoints; i++)
			{
				pts[i] = *(CvPoint*) cvGetSeqElem(contourPoints, i); //convert CvSeq to an array of points
				//std::cout << "Point " << i << "(" << pts[i].x << "," << pts[i].y << ")\n";
			}

			for (int i=0; i<numPoints; i++)
			{
				if (i>0)
				{
					cvLine(frame, pts[i], pts[i-1], CV_RGB(0,150,255), 2, 8, 0);
				}
				else
				{
					cvLine(frame, pts[i], pts[numPoints-1], CV_RGB(0,150,255), 2, 8, 0);
				}
			}


			//find top two points
			int minY = 10000;
			int yVal = 0;

			int top2Points[2] = {0, 1};

			for (int i=0; i<numPoints; i++)
			{
				yVal = pts[i].y;
				if ( yVal < minY)
				{
					top2Points[0] = i;
					minY = yVal;
				}
			}

			minY = 9000;

			for (int i=0; i<numPoints; i++)
			{
				yVal = pts[i].y;
				if (yVal < minY && yVal > pts[top2Points[0]].y)
				{
					top2Points[1] = i;
					minY = yVal;
				}
			}


			if (pts[top2Points[0]].x < pts[top2Points[1]].x)
			{
				topLeftIndex = top2Points[0];
			}
			else
			{
				topLeftIndex = top2Points[1];
			}


			cvCircle(frame, pts[topLeftIndex], 8, colorYellow, 2, 8, 0);

			CvPoint topRightPoint;
			CvPoint bottomLeftPoint;

			if (topLeftIndex == 0)
			{
				topRightPoint = pts[numPoints-1];
			}
			else
			{
				topRightPoint = pts[topLeftIndex-1];
			}

			if (topLeftIndex == (numPoints-1))
			{
				bottomLeftPoint = pts[0];
			}
			else
			{
				bottomLeftPoint = pts[topLeftIndex+1];
			}


			cvLine(frame, pts[topLeftIndex], topRightPoint, colorRed, 5, 8, 0);
			cvLine(frame, pts[topLeftIndex], bottomLeftPoint, colorBlue, 5, 8, 0);



		}
		contours = contours->h_next;
	}
#ifdef DEBUG
	CvPoint centerPoint = cvPoint(cvGetSize(frame).width/2, cvGetSize(frame).height/2);
	cvCircle(frame, centerPoint , 1, colorGreen, 2, 8, 0);
	cvLine(frame, cvPoint(centerPoint.x-5, centerPoint.y-5), cvPoint(centerPoint.x+5, centerPoint.y+5), colorGreen, 1, 8, 0);
	cvLine(frame, cvPoint(centerPoint.x-5, centerPoint.y+5), cvPoint(centerPoint.x+5, centerPoint.y-5), colorGreen, 1, 8, 0);

	cvShowImage("Polys", frame);
#endif
}


int main()
{
	CvCapture* capture = NULL;
#ifdef DEBUG
	cvNamedWindow("Input Image", 1);
	cvNamedWindow("Threshold Settings", 1);
	cvCreateTrackbar("Hue min", "Threshold Settings", &hueMin, 255, NULL);
	cvCreateTrackbar("Hue max", "Threshold Settings", &hueMax, 255, NULL);
	cvCreateTrackbar("Sat min", "Threshold Settings", &satMin, 255, NULL);
	cvCreateTrackbar("Sat max", "Threshold Settings", &satMax, 255, NULL);
	cvCreateTrackbar("Val min", "Threshold Settings", &valMin, 255, NULL);
	cvCreateTrackbar("Val max", "Threshold Settings", &valMax, 255, NULL);
	cvCreateTrackbar("Area threshold", "Threshold Settings", &areaValue, 5000, NULL);
	cvCreateTrackbar("Bounding Threshold", "Threshold Settings", &boundingBoxSearchTolerance, 100, NULL);
#endif





	//grab frame from camera
	capture = cvCaptureFromAVI("./IMG_022poop4.MOV"); //put file name here
	if (capture == NULL)
	{
		std::cout << "Capturing from camera.\n";
		capture = cvCaptureFromCAM(1);
		cvSetCaptureProperty(capture, CV_CAP_PROP_BRIGHTNESS, 0.00784); //2 in GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_CONTRAST, 0.2); //51 in GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_SATURATION, 0.498); //127 in GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_GAIN, 0); //0 in GUVC view
		cvSetCaptureProperty(capture, CV_CAP_PROP_AUTO_EXPOSURE, 0); //Have to set through GUVCview as 0, manual mode !!!!
		cvSetCaptureProperty(capture, CV_CAP_PROP_TEMPERATURE, 10000); //Have to set through GUVCview as 10000 !!!
		cvSetCaptureProperty(capture, CV_CAP_PROP_EXPOSURE, 166); //Have to set through GUVCview as 166 !!!
		cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, 224); //Have to set through GUVCview as 224 !!!
		//Backlight compensation 1, have to set through GUVCview
		cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 640);
		cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 480);
	}

	frame = cvQueryFrame(capture);


	findStuff(1);
	cvSaveImage("TestImage.png", frame);
	cvWaitKey(0);

	while (true)
	{
		frame = cvQueryFrame(capture);
#ifdef DEBUG
		cvShowImage("Input Image", frame);
#endif

		if (!frame)
		{
			std::cout << "ERROR: Couldn't get a frames.\n";
		}
		double tick = getTickCount();
		findStuff(1);
		std::cout << "Time taken:" << (getTickCount() - tick) / getTickFrequency() * 1000 << " ms. \n";

		int c = cvWaitKey(27);
		if (c == 27)
		{
			break;
		}

	}
	cvReleaseImage(&frame);
}

